#include "ColorSpaceConvert.h"
#include <cstring>

namespace AVSAnalyzer 
{
    unsigned char ColorSpaceConvert::clipValue(unsigned char x, unsigned char min_val, unsigned char max_val) 
    {
        if (x > max_val) 
        {
            return max_val;
        }
        else if (x < min_val) 
        {
            return min_val;
        }
        else 
        {
            return x;
        }
    }

    bool ColorSpaceConvert::bgr24ToYuv420p(unsigned char* bgrBuf, int w, int h, unsigned char* yuvBuf) 
    {
        unsigned char* ptrY, * ptrU, * ptrV, * ptrRGB;
        memset(yuvBuf, 0, w * h * 3 / 2);
        ptrY = yuvBuf;
        ptrU = yuvBuf + w * h;
        ptrV = ptrU + (w * h * 1 / 4);
        unsigned char y, u, v, r, g, b;

        for (int j = 0; j < h; ++j) 
        {
            ptrRGB = bgrBuf + w * j * 3;
            for (int i = 0; i < w; i++) 
            {
                b = *(ptrRGB++);
                g = *(ptrRGB++);
                r = *(ptrRGB++);

                y = (unsigned char)((66 * r + 129 * g + 25 * b + 128) >> 8) + 16;
                u = (unsigned char)((-38 * r - 74 * g + 112 * b + 128) >> 8) + 128;
                v = (unsigned char)((112 * r - 94 * g - 18 * b + 128) >> 8) + 128;
                *(ptrY++) = clipValue(y, 0, 255);
                if (j % 2 == 0 && i % 2 == 0) 
                {
                    *(ptrU++) = clipValue(u, 0, 255);
                }
                else 
                {
                    if (i % 2 == 0) 
                    {
                        *(ptrV++) = clipValue(v, 0, 255);
                    }
                }
            }
        }
        return true;
    }

    bool ColorSpaceConvert::bgr24ToNv12(unsigned char* bgrBuf, int w, int h, unsigned char* nv12Buf) 
    {
        unsigned char* yPtr = nv12Buf;
        unsigned char* uvPtr = nv12Buf + (w * h);
        
        for (int j = 0; j < h; j++) 
        {
            for (int i = 0; i < w; i++) 
            {
                int index = j * w * 3 + i * 3;
                unsigned char b = bgrBuf[index];
                unsigned char g = bgrBuf[index + 1];
                unsigned char r = bgrBuf[index + 2];
                
                // RGB to Y
                int y = ((66 * r + 129 * g + 25 * b + 128) >> 8) + 16;
                yPtr[j * w + i] = clipValue(y, 0, 255);
                
                // RGB to UV
                if (j % 2 == 0 && i % 2 == 0) 
                {
                    int u = ((-38 * r - 74 * g + 112 * b + 128) >> 8) + 128;
                    int v = ((112 * r - 94 * g - 18 * b + 128) >> 8) + 128;
                    uvPtr[(j * w / 2) + i] = clipValue(u, 0, 255);
                    uvPtr[(j * w / 2) + i + 1] = clipValue(v, 0, 255);
                }
            }
        }
        return true;
    }
} 